package it.unicam.cs.cosy.bioshape.core.components;

import it.unicam.cs.cosy.bioshape.core.components.movers.EntityDriver;
import it.unicam.cs.cosy.bioshape.core.components.services.Service;
import it.unicam.cs.cosy.bioshape.core.shapes.BoundingBox;
import it.unicam.cs.cosy.bioshape.core.shapes.BoundingSphere;
import it.unicam.cs.cosy.bioshape.core.shapes.Bounds;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Tuple3d;
import javax.vecmath.Vector3d;

import vclip.ConvexPolyhedron;
import vclip.Matrix4dX;
import vclip.PolyTree;
import vclip.Vertex;

/**
 * Implements a simulation {@code Entity}. The geometrical representation is
 * managed through V-Clip {@code PolyTree} objects which are particularly useful
 * for collision detection/collision response tasks.
 * 
 * @author Carmine Oliva, Federico Buti
 */
public class ShapedEntity extends PolyTree implements Entity {

    // TODO PREVEDERE UNA STRUTTURA INTERNA CHE TENGA TRACCIA DEI BIND
    // EFFETTUATI: ALTRIMENTI SI RISCHIA CHE BEHAVIOUR MALFORMATI ESEGUANO SPLIT
    // INESISTENTI! INVECE SE TENGO TRACCIA ELIMINO UNA VOLTA ESEGUITA LA SPLIT

    // TODO PREVEDERE UN METODO POST-SPLIT CHIAMATO IN AUTOMATICO, O DALLA SHAPE
    // O DA LOCALCOORDINATOR: CI PERMETTE UN TUNING PIU' FINO IN POST SPLIT!!

    // /** vector representing the center of the Entity */
    // protected Vector3d center;
    // TODO aggiungere una coppia di matrici di appoggio: tutte le operazioni le
    // facciamo su quelle così da non creare oggetti aggiuntivi. Magari le
    // aggiungiamo sul LC, così da evitare di generare matrici per ogni singola
    // shape!!
    
    /** auto-generated serial ID */
    private static final long serialVersionUID = 8765242684046212725L;
    
    /**
     * Determines how many iterations the shape waits, after collided, to change
     * again its velocities by the driver
     */
    public static final int NO_UPDATE_VELOCITY_AFTER_COLLIDED = 1;

    /** Unique identifier for the entity */
    protected long id;

    /** Mass of the entity, useful to calculate the correct impulse response */
    protected double mass;

//    /** {@code true} if in the PREVIOUS iteration the entity has collided */
//    private boolean collided = false;

    /** The bounding volume associated to the entity, usually an AABB */
    private Bounds bounds;

    /** The perception radius, used in non-collision driven interactions */
    private double perceptionRadius;

    // TODO Documentare
    protected ShapeRequest request;

    /** Velocity vector for instantVelocity of the entity */
    private Vector3d instantVelocity = new Vector3d();

    /**
     * Angular velocity pseudovector for instantAngularVelocity of the entity
     * (automatically converted in radians) - Used Rotation Representation:
     * fixed axis roll, pitch, yaw about X, Y, Z axes respectively, typical for
     * angular velocities
     */
    private Vector3d instantAngularVelocity = new Vector3d();

    /** Driver associated to the entity, calculate next velocities */
    private EntityDriver entityDriver;

    /** Stores information about perceived environment */
    private ArrayList<Pair<Class<?>, Object>> perceivedEnvironment = new ArrayList<Pair<Class<?>, Object>>();

    /**
     * Stores the original name of the geometry used to construct the shape of
     * this entity, as given in the input file
     */
    private String geometry;

    private int iterationsAfterCollided;

    /**
     * Builds an entity with a shape, a mass and all the other information to
     * set it in the simulated environment. Translation and
     * instantAngularVelocity velocities are set to (0,0,0) at the beginning.
     * They will be updated through the {@code EntityDriver} object.
     * 
     * @param id
     *            unique identified for the shape
     * @param mass
     *            mass of the shape
     * @param shape
     *            {@code PolyTree} object representing the shape of the entity
     * @param boundType
     *            Type of bounding volume: sphere or AABB
     * @param perception
     *            The perception radius associated to the entity
     * @param position
     *            {@code Point3d} representing the position of the shape in the
     *            simulated space
     * @param ed
     *            {@code EntityDriver} instance associated to the shape to
     *            update its velocities along the simulation
     */
    public ShapedEntity(long id, double mass, PolyTree shape, String boundType,
            double perception, Vector3d position, EntityDriver ed) {
        super(Entity.SHAPE_NAME_PREFIX + id, shape);
        this.buildAllBoundingHulls(OBB_HULL);
        this.id = id;
        geometry = shape.getName();
        this.mass = mass;
        this.iterationsAfterCollided = 0;
        // calculate the center of mass for entities not in the origin
        Vector3d center = new Vector3d();
        centerOfMass(center);
        position.sub(center); // TODO controllare ADD forse è SUB!
        this.getTransform().setTranslation(position);
        createBoundingVolume(boundType);
        setPerceptionRadius(perception);
        entityDriver = ed;
        setServiceRequest();
    }

    /**
     * Builds an entity with a shape, a mass and a bounding shape. Such entity
     * is <i>limited</i> w.r.t. entities created with the other constructor: it
     * can neither move, nor perceive. Setting an infinite mass we can obtain
     * <i>almost</i> the behaviour of a {@code WallEntity} object.
     * 
     * @param id
     *            unique identified for the shape
     * @param mass
     *            mass of the shape
     * @param shape
     *            {@code PolyTree} object representing the shape of the entity
     * @param bounds
     *            Type of bounding volume: sphere or AABB
     */
    public ShapedEntity(long id, double mass, ConvexPolyhedron shape,
            Bounds bounds) {
        super(Entity.SHAPE_NAME_PREFIX + id, shape);
        this.buildAllBoundingHulls(OBB_HULL);
        this.id = id;
        this.mass = mass;
        this.bounds = bounds;
        this.iterationsAfterCollided = 0;
        setServiceRequest();
    }

    private void createBoundingVolume(String b) {
        if (b.equals("aabb"))
            bounds = new BoundingBox(getCenter(), this.radius());
        else
            bounds = new BoundingSphere(getCenter(), this.radius());
    }

    @Override
    public void move(double deltaT) {
        Matrix4d transform = getTransform();
        // instantVelocity
        double x = transform.m03 + instantVelocity.x * deltaT;
        double y = transform.m13 + instantVelocity.y * deltaT;
        double z = transform.m23 + instantVelocity.z * deltaT;
        // instantAngularVelocity
        Matrix3d currRot = new Matrix3d();
        transform.get(currRot);
        Matrix4dX tMatrix = new Matrix4dX();
        tMatrix.setRpy(instantAngularVelocity.z * deltaT,
                instantAngularVelocity.y * deltaT, instantAngularVelocity.x
                        * deltaT);
        Matrix3d newRot = new Matrix3d();
        tMatrix.get(newRot);
        newRot.mul(currRot);
        transform.set(newRot);
        transform.m03 = x;
        transform.m13 = y;
        transform.m23 = z;
        // System.out.println("Moving shape " + getName());
        // System.out.println(this);
        // System.out.println("DeltaT " + deltaT);
        // System.out.println("Instant Velocity " + instantVelocity);
        // System.out.println("Instant Angular Velocity " +
        // instantAngularVelocity);
        // System.out.println("Transform " + transform);
        // if (isAtomic())
        // setTransform(transform); // TODO aggiunto per il bind! Luca: ??
        // else
        // LUCA: NOOOOOOOOO: this.setTransform(transform); altrimenti se è un PolyTree
        // composto sposta tutte le sottocomponenti!!!! \°o°/
        // Luca: il seguito è inutile, non lo fa automaticamente il polytree?

        // for (Iterator<PolyTree> iP = this.getComponents(); iP.hasNext();) {
        // PolyTree p = iP.next();
        // // System.out.println(p.getName());
        // Matrix4dX m = new Matrix4dX(p.getTransform());
        // m.mulTrans(this.getTransform(), m);
        // // System.out.println(m);
        // // p.setTransform(m);
        // }
    }

    @Override
    public Bounds getBounds() {
        return bounds;
    }

    /*
     * Calculates the REFERENCE center starting from the traslation vector of
     * the transformation matrix by adding the position of the polyhedron
     * center. If the polyhedron has center in (0,0,0) the traslation in the
     * transformation matrix corresponds exactly to the current reference
     * center.
     */
    @Override
    public Vector3d getCenter() {
        Vector3d trans = new Vector3d();
        Vector3d center = new Vector3d();
        this.getTransform().get(trans);
        this.centerOfMass(center);
        trans.add(center);
        return trans;
    }

    @Override
    public long getId() {
        return id;
    }

    @Override
    public double getMass() {
        return mass;
    }

    @Override
    public boolean isCollided() {
        return iterationsAfterCollided > 0;
    }

    @Override
    public void ncdInteractions() {
        // TODO
    }

    @Override
    public ShapeRequest queryServices() {
        request.setCenter(bounds.getCenter());
        perceivedEnvironment.clear(); // basic behaviour ---> takes everything
                                      // perceptible.
        return request;
    }

    @Override
    public void setCenter(Tuple3d center) {
        // TODO rivedere, forse togliere
        center.set(center);
    }

    @Override
    public void setCollided(boolean state) {
        if (state)
            iterationsAfterCollided = ShapedEntity.NO_UPDATE_VELOCITY_AFTER_COLLIDED;
        else iterationsAfterCollided = 0;
    }

    @Override
    public void setInstantVelocity(Vector3d vector) {
        instantVelocity.set(vector);
    }

    /*
     * The default version of this method takes the current instantVelocity and
     * instantAngularVelocity velocities and pass them to the {@code
     * EntityDriver} instance associated with this entity.
     */
    @Override
    public void updateVelocity() {
        if (iterationsAfterCollided > 0)
            iterationsAfterCollided--;
        else {
        Object parameter[] = { instantVelocity };
        instantVelocity = entityDriver.calculateNewInstantVelocity(parameter);
        Object parameter1[] = { instantAngularVelocity };
        instantAngularVelocity = entityDriver
                .calculateNewInstantAngularVelocity(parameter1);
        }
    }



    @Override
    public Vector3d getInstantVelocity() {
        return new Vector3d(instantVelocity);
    }

    @Override
    public void setInstantAngularVelocity(Vector3d rot) {
        instantAngularVelocity.set(rot);
    }

    @Override
    public Vector3d getInstantAngularVelocity() {
        return new Vector3d(instantAngularVelocity);
    }


    // // TODO: ci serve sto metodo? È utile?????
    // /**
    // * Metodo necessario per ottenere i vertici del polytree. Il metodo è
    // * presente anche nel pacchetto di vclip tuttavia, poichè trasforma in
    // * poliedro non prende i punti effettivi Evita stessi vertici ripetuti e
    // può
    // * essere usato nel momento in cui bisogna calcolare il centro Ottengo un
    // * arraylist in quanto a priori non so quanti vertici sono presenti
    // *
    // * @return l'array di vertici del polytree
    // *
    // */
    // public ArrayList<Point3d> getVertexOfPolytree() {
    // // CREAZIONE ARRAY CHE RAPPRESENTA L'OUTPUT
    // ArrayList<Point3d> risultato = new ArrayList<Point3d>();
    // // OTTENGO L'ITERATORE CHE SERVE A SCORRERE LA LISTA DI POLYTREE INTERNI
    // // DELL'OGGETTO polytree (var istanza)
    // Iterator<?> iteratore = this.getComponents();
    // // SCORRO LA LISTA DI POLYTREE INTERNI
    // while (iteratore.hasNext()) {
    // // OTTENGO IL SINGOLO ELEMENTO DELLA LISTA VOLTA VOLTA
    // PolyTree polyInterno = (PolyTree) iteratore.next();
    // // OTTENGO L'ARRAY DEI VERTICI (N.B. PER ASSUNZIONE ABBIAMO CHE OGNI
    // // POLYTREE INTERNO E' CONVESSO)
    // Vertex vertici[] = polyInterno.getPolyhedron().getVerts();
    // // SCORRO I VERTICI DEL SINGOLO POLYEDRO
    // for (int i = 0; i < vertici.length; i++) {
    // // VARIBILE CHE SERVE A DETERMINARE SE RISCONTRATO 1 ELEMENTO =
    // boolean controllo = false;
    //
    // // SCORRO I VERTICI GIA' INSERITI
    // for (int j = 0; j < risultato.size(); j++) {
    // // SE OGNI COORDINATA COORRISPONDE NON DEVO AGGIUNGERE IL
    // // VERTICE ALL'ARRAY
    // if ((vertici[i].getCoords().getX() == risultato.get(j)
    // .getX())
    // && (vertici[i].getCoords().getY() == risultato.get(
    // j).getY())
    // && (vertici[i].getCoords().getZ() == risultato.get(
    // j).getZ())) {
    //
    // // HO TROVATO 1 ELEMENTO =
    // controllo = true;
    // break;
    // }
    // }
    // // SE controllo E' RIMASTO = false, IL VERTICE DEVE ESSERE
    // // AGGIUNTO
    // if (controllo == false)
    // // AGGIUNGO IL PUNTO ALL'ARRAYLIST
    // risultato.add(new Point3d(vertici[i].getCoords().getX(),
    // vertici[i].getCoords().getY(), vertici[i]
    // .getCoords().getZ()));
    // // System.out.println("il punto aggiunto ha coordinate: " +
    // // vertici[i].getCoords().getX() +" ," +
    // // vertici[i].getCoords().getY() + " ," +
    // // vertici[i].getCoords().getZ());
    // }
    // }
    // // RICHIAMO L'ARRAYLIST OTTENUTO
    // return risultato;
    // }

    // Luca: Al momento non viene mai chiamato!!!! O_O
    @Override
    public void join(Entity e) {
//        if (!(e instanceof ShapedEntity))
//            throw new IllegalArgumentException("Binding is possible only among"
//                    + " ShapedEntity objects");
//        ShapedEntity se = (ShapedEntity) e;
//        // TODO codice per esempio Luca: O_O come per esempio?
//        Vector3d newC = new Vector3d();
//        newC.add(getCenter());
//        newC.add(se.getCenter());
//        newC.scale(0.5);
//        Vector3d v1 = new Vector3d(getCenter());
//        Vector3d v2 = new Vector3d(se.getCenter());
//        v1.sub(newC, v1); // offset
//        v2.sub(newC, v2); // offset
//        System.out.println("VV1" + v1);
//        System.out.println("VV2" + v2);
//        Matrix4d m1 = new Matrix4d(this.getTransform());
//        m1.setTranslation(v1);
//        this.buildBoundingHull();
//        addComponent(getName(), this, m1);
//        Matrix4d m2 = new Matrix4d(se.getTransform());
//        m2.setTranslation(v2);
//        se.buildBoundingHull();
//        addComponent(se.getName(), se, m2);
//        this.buildBoundingHull();
//        Matrix4d m = new Matrix4d();
//        m.setIdentity();
//        m.setTranslation(newC);
//        this.setTransform(m);  // Luca: ERRORE!
    }

    @Override
    public void split() {
        // TODO verificare che sia atomic o meno e quindi aggiornare la flag
    }

    @Override
    public void setDriver(EntityDriver entityDriver) {
        this.entityDriver = entityDriver;
    }

    // TODO Documentare!
    protected void setServiceRequest() {
        request = new ShapeRequest(perceptionRadius, bounds.getCenter(), null);
        // perception key is simply "Service" while operation is null!!
        request.addServiceEntry(Service.class, null, perceivedEnvironment);
    }

    /**
     * @return the perceptionRadius
     */
    public double getPerceptionRadius() {
        return perceptionRadius;
    }

    /**
     * @param perceptionRadius
     *            the perceptionRadius to set
     */
    public void setPerceptionRadius(double perceptionRadius) {
        this.perceptionRadius = perceptionRadius;
    }

    @Override
    public String getGeometry() {
        return geometry;
    }

    @Override
    public List<PolyTree> getComponentEntities() {
        ArrayList<PolyTree> results = new ArrayList<PolyTree>();
        if (super.getComponents().hasNext() == false)
            return results;
        for (Iterator<PolyTree> i = super.getComponents(); i.hasNext();) {
            // Object o = i.next();
            PolyTree o = i.next();
            // if (o instanceof ShapedEntity) { // TODO viene visto come
            // polytree
            // results.add((Entity) o);
            // results.addAll(((Entity) o).getComponentEntities());
            results.add(o);
            // results.addAll(o.getComponentEntities());
            // }
        }
        return results;
    }

    @Override
    public boolean isAtomicEntity() {
        return getComponentEntities().size() == 0;
    }

    @Override
    public Matrix3d getInertiaTensor() {
        Matrix3d m1 = new Matrix3d();
        inertiaTensor(m1);
        m1.mul(mass / volume());
        return m1;
    }

    /* (non-Javadoc)
     * @see java.lang.Object#hashCode()
     */
    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        result = prime * result + (int) (id ^ (id >>> 32));
        return result;
    }

    /* (non-Javadoc)
     * @see java.lang.Object#equals(java.lang.Object)
     */
    @Override
    public boolean equals(Object obj) {
        if (this == obj)
            return true;
        if (obj == null)
            return false;
        if (!(obj instanceof ShapedEntity))
            return false;
        ShapedEntity other = (ShapedEntity) obj;
        if (id != other.id)
            return false;
        return true;
    }
    
}